#pragma once
#include "rclcpp/rclcpp.hpp"
#include "tf2/exceptions.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "base_interfaces/msg/armors.hpp"
#include "base_interfaces/msg/kinematic_status.hpp"
#include "base_interfaces/msg/kinematic_status_of_all.hpp"
#include "Aimer.hpp"
#include "KinematicModel.hpp"
#include "ModelObserver.hpp"

using std::placeholders::_1;

namespace wmj
{
    /**
     * @class TopAimer
     * @brief 小陀螺ekf建模节点。
     * @author 吴奇优
     */
    class TopAimer : public rclcpp::Node
    {
    public:
        /**
         * @brief TopAimer类构造函数
         * @param name 节点名称
         */
        explicit TopAimer(std::string name);

    private:
        
        rclcpp::Subscription<base_interfaces::msg::Armors>::SharedPtr m_armors_sub;             ///< Armors订阅者
        rclcpp::Publisher<base_interfaces::msg::KinematicStatusOfAll>::SharedPtr m_status_pub;  ///< 卡尔曼模型发布者
        rclcpp::Publisher<base_interfaces::msg::Armors>::SharedPtr m_debug_observer;
        int m_input_width = 800;                                    ///< setDeepROISize参数
        int m_input_height = 448;
        int m_detect_id_num;                                        ///< 识别的id数量
        int m_top_aimer_instance_num;                               ///< top_aimer实例数量
        bool m_ekf_on;                                              ///< 是否开启ekf建模，不建模直接传递装甲板
        std::map<int, int> mapping;                                 ///< id至top_aimer实例下标的映射
        bool m_debug;
        std::vector<std::shared_ptr<wmj::Aimer>> m_top_aimer;       ///< top_aimer实例
        std::vector<std::shared_ptr<KinematicModel>> m_model;       ///< ekf模型
        std::unique_ptr<tf2_ros::Buffer> tf_buffer_;                ///< TF2缓冲区
        std::shared_ptr<tf2_ros::TransformListener> tf_listener_;   ///< TF2监听器
        int m_isdetected = 0;
    
    private:
        /**
         * @brief Armors的回调函数
         * @param armors armors的msg消息
         */
        void armors_cb(base_interfaces::msg::Armors::SharedPtr armors);
        
        /**
         * @brief 读入参数文件
         * @param file_path 文件路径
         */
        void setParam(const std::string &file_path);
        
        /**
         * @brief tf2获取坐标变化
         * @param tar_frame 目标坐标系
         * @param src_frame 源坐标系
         * @param stamp 时间戳
         * @param transform 返回的坐标变化
         * @return 是否成功获取转换
         */
        bool getTransform(std::string tar_frame, std::string src_frame, builtin_interfaces::msg::Time stamp, geometry_msgs::msg::TransformStamped &transform);
        
        /**
         * @brief 将模型转换为kinematicStatus消息
         * @param status 模型
         * @param msg 转化后的kinematicStatus消息
         * @param armors 识别到的Armors
         * @return 是否成功转换
         */
        bool model2msg(std::shared_ptr<wmj::KinematicModel> status, base_interfaces::msg::KinematicStatus &msg, Armors armors);
    };
}